% Devin Koepl

function theta = equilibrium_angle(ktom, l0, h, vx, theta)
    g = 9.81;
    m = 1;
    k = m * ktom;
   
    ri = 1;
    dri = 2;
    oi = 3;
    doi = 4;
    
    P = 0.1;
 
    %Setup ODE solver options with error tolerances and event detection.
    refine = 4;
    tol = 1e-6;
    options = odeset('Events', @events, 'Refine', refine, 'RelTol',tol,'AbsTol', repmat(tol, 1, doi));
    
    c = 0;
    while (c < 1e2)
        c = c + 1;
    
        y = l0 * sin(theta);
        vy = - sqrt(2 * g * (h - y));
        R = [cos(theta), -sin(theta) ; sin(theta), cos(theta)];
        v = R' * [vx; vy];
        vr = v(1);
        vo = v(2) / l0;

        ic = zeros(1, 4);
        ic(ri) = l0;
        ic(dri) = vr;
        ic(oi) = theta;
        ic(doi) = vo;

        [t, y, te, ye, ie] = ode15s(@model, [0 inf], ic, options);
        
        error = pi - y(1, oi) - y(end, oi);
        
        if (abs(error) < tol*100)
            return;
        end
        
        theta = theta + P * error;
        
    end
    
    theta = NaN;
    
    function yp = model(t, y)  
        yp = zeros(length(ic), 1);
        yp(ri) = y(dri);
        yp(dri) = (k/m) * (l0 - y(ri)) + y(ri) * y(doi)^2 - g * sin(y(oi));
        yp(oi) = y(doi);
        yp(doi) = ( -2 * y(dri) * y(doi) - g * cos(y(oi)) ) / y(ri);     
    end
    
    function [value,isterminal,direction] = events(t, y)             
        value = [y(oi) - pi; y(oi); l0 - y(ri)];
        isterminal = [1; 1; 1];   
        direction = [1; -1; -1];   
    end

end